#pragma once

#include "g2o/core/base_multi_edge.h"
#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_vec.hpp"
#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_q.hpp"
#include "lidar_localization/models/graph_optimizer/preintegration/preintegration.hpp"

namespace g2o
{
    class EdgeOdom : public BaseMultiEdge<6, std::pair<Eigen::Vector3d, Eigen::Quaterniond>>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        EdgeOdom();
        virtual bool read(std::istream &is) { return false; }
        virtual bool write(std::ostream &os) const { return false; }
        void computeError();
        virtual void linearizeOplus();
        virtual void setMeasurement(const std::pair<Eigen::Vector3d, Eigen::Quaterniond> &m)
        {
            _measurement = m;
        }
        std::vector<g2o::MatrixX::MapType, Eigen::aligned_allocator<g2o::MatrixX::MapType>> GetJacobian() const
        {
            return _jacobianOplus;
        }
    };
} // namespace g2o